package com.nick.robots;
import java.util.Random;
import java.util.Scanner;

import com.example.androidmaze.Constants;



public class CuriousGamblerDriver implements RobotDriver {
	Scanner reader = new Scanner(System.in);
	private Robot robot;
	boolean canMoveLeft = false;
	boolean canMoveRight = false;
	boolean canMoveFwd = false;
	
	public CuriousGamblerDriver(CuriousGamblerRobot r)
	{
		this.robot = r;
	}
	
	@Override
	public void setRobot(Robot r) throws UnsuitableRobotException {
		robot = r;
		float robotBattLevel = robot.getCurrentBatteryLevel();
		float robotRotationCost = robot.getEnergyForFullRotation();
		float robotMoveCost = robot.getEnergyForStepForward();
		if ((robotBattLevel == 2500) && (robotRotationCost == 4) && (robotMoveCost == 5)) {
			try {
				robot.distanceToObstacleAhead();
				//robot.distanceToObstacleBehind();
				robot.distanceToObstacleOnLeft();
				//robot.distanceToObstacleOnRight();
				robot.canSeeGoalAhead();
				//robot.canSeeGoalBehind();
				robot.canSeeGoalOnLeft();
				//robot.canSeeGoalOnRight();
			}
			catch (UnsupportedMethodException e) {
				throw new UnsuitableRobotException();
			}
		}
		
	}

	@Override
	public boolean drive2Exit() throws Exception {
		Random random = new Random();

		while (!robot.hasStopped() || !robot.isAtGoal()) {
			if (robot.hasStopped()) {
				throw new Exception();
			}
			else if (robot.isAtGoal()) {
				return true;
			}
			
			//start algorithm

			updateSensors();
			
			if (canMoveFwd) {
				while (robot.distanceToObstacleAhead() > 0) {
					robot.move(1, Constants.forwards);
					updateSensors();
					
					//if we reach an intersection
					if (canMoveLeft && canMoveRight && (robot.distanceToObstacleAhead() > 0)) {
						//pick random direction of 3
						int moveDirection = random.nextInt(3);
						//find direction
						switch (moveDirection) {
							//turn left
							case 0:
								robot.rotate(Constants.LEFT_ROTATE);
								break;
							//turn right
							case 1:
								robot.rotate(Constants.RIGHT_ROTATE);
								break;
							//go straight 1 step, will kick back and keep going
							case 2:
								robot.move(1, Constants.forwards);
								break;
							default:
								break;
							}
					}
					//if we reach a T split
					else if (canMoveLeft && canMoveRight) {
						//pick random direction of 2
						int moveDirection = random.nextInt(2);
						//find direction
						switch (moveDirection) {
							//turn left
							case 0:
								robot.rotate(Constants.LEFT_ROTATE);
								break;
							//turn right
							case 1:
								robot.rotate(Constants.RIGHT_ROTATE);
								break;
							default:
								break;
						}
					}
					//if we reach a forwards & L|R split
					else if ((canMoveLeft || canMoveRight) && robot.distanceToObstacleAhead() > 0) {
						//pick random direction of 2
						int moveDirection = random.nextInt(2);
						//find direction
						switch (moveDirection) {
						//turn left|right
							case 0:
								if (canMoveLeft) {
									robot.rotate(Constants.LEFT_ROTATE); }
								else {
									robot.rotate(Constants.RIGHT_ROTATE); }
								break;
							case 1:
								robot.move(1, Constants.forwards);
								break;
							default:
								break;
						}
					}
					//if we reach a turn
					else if (canMoveLeft || canMoveRight) {
						if (canMoveLeft) {
							robot.rotate(Constants.LEFT_ROTATE);
						}
						if (canMoveRight) {
							robot.rotate(Constants.RIGHT_ROTATE);
						}
					}
				}
			}
			else {
				// can't more forwards, turn
				robot.rotate(Constants.RIGHT_ROTATE);
			}
		}
		//returns false otherwise
		return false;
	}
	
	public void updateSensors() {
		try {
			canMoveFwd = (robot.distanceToObstacleAhead() > 0);
			canMoveLeft = (robot.distanceToObstacleOnLeft() > 0);
			canMoveRight = (robot.distanceToObstacleOnRight() > 0);
		} catch (UnsupportedMethodException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
	}

	@Override
	public float getEnergyConsumption() {
		return (2500 - robot.getCurrentBatteryLevel());
	}

	@Override
	public int getPathLength() {
		return robot.getDistTraveled();
	}

	
}
